﻿
// 2024/2/23: 首个版本

#ifndef VEHICLE_CONTROL_V1_H
#define VEHICLE_CONTROL_V1_H

#include "spadas.h"

namespace vehicle_control_v1
{
	using namespace spadas;

	enum class LongitudinalControlMode
	{
		NoRequest = 0, // Not requesting longitudinal control / 不请求纵向控制
		Position = 1, // Control throttle and brake pedal, value ranges from -1(max brake) ~ 1(max throttle) / 控制油门、刹车踏板，数值范围为-1(刹车到底)~1(油门到底)
		Acceleration = 2, // Control acceleration and deceleration, in m/s² units (The target absolute speed is necessary to set) / 控制加、减速度，数值单位为m/s²，同时需要指定目标绝对速度
	};

	enum class LateralControlMode
	{
		NoRequest = 0, // Not requesting lateral control / 不请求横向控制
		Position = 1, // Control steer, value ranges from -1(max right turning) ~ 1(max left turning) / 控制方向盘，数值范围为-1(右打到底)~1(左打到底)
		Curvature = 2, // Control turning curvature, in 1/m units / 控制转弯曲率，数值单位为1/m
	};

	enum class GearControlMode
	{
		NoRequest = 0, // Not requesting gear switching / 不请求换挡
		Drive = 1, // Switch to drive gear / 请求换D挡
		Reverse = 2, // Switch to reverse gear / 请求换R挡
	};

	struct VehicleControl
	{
		FullTimestamp timestamp; // 时间戳

		LongitudinalControlMode longitudinalMode; // 纵向控制模式
		Double longitudinalValue; // 纵向控制数值
		Double targetSpeed; // [KPH] Target absolute speed, only available on acceleration control mode / 目标绝对速度，仅限加速度控制模式下有效

		LateralControlMode lateralMode; // 横向控制模式
		Double lateralValue; // 横向控制数值

		GearControlMode gearMode; // 换挡控制模式

		VehicleControl()
		{
			longitudinalMode = LongitudinalControlMode::NoRequest;
			longitudinalValue = 0;
			targetSpeed = 0;

			lateralMode = LateralControlMode::NoRequest;
			lateralValue = 0;

			gearMode = GearControlMode::NoRequest;
		}

		static String getProtocolName(UInt channel)
		{
			if (channel == UINF) return "vehicle-control-v1";
			else return (String)"vehicle-control-v1@" + channel;
		}

		static Bool supportInterpolation()
		{
			return FALSE;
		}

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp = timestamp;
			sample.values = Array<GeneralElement>(6);
			GeneralElement *data = sample.values.data();

			data[0] = (Double)(Int)longitudinalMode;
			data[1] = longitudinalValue;
			data[2] = targetSpeed;
			data[3] = (Double)(Int)lateralMode;
			data[4] = lateralValue;
			data[5] = (Double)(Int)gearMode;

			sample.significantCount = sample.values.size();
			return sample;
		}

		Bool fromGeneralSample(String protocol, SessionGeneralSample sample)
		{
			timestamp = sample.timestamp;

			if (protocol == "vehicle-control-v1")
			{
				if (sample.values.size() != 6) return FALSE;

				longitudinalMode = sample.values[0].valid ? (LongitudinalControlMode)(Int)sample.values[0].value : LongitudinalControlMode::NoRequest;
				longitudinalValue = sample.values[1].value;
				targetSpeed = sample.values[2].value;
				lateralMode = sample.values[3].valid ? (LateralControlMode)(Int)sample.values[3].value : LateralControlMode::NoRequest;
				lateralValue = sample.values[4].value;
				gearMode = sample.values[5].valid ? (GearControlMode)(Int)sample.values[5].value : GearControlMode::NoRequest;

				return TRUE;
			}

			return FALSE;
		}

		static VehicleControl interpolate(VehicleControl& s1, Double w1, VehicleControl& s2, Double w2, FullTimestamp timestamp)
		{
			return VehicleControl();
		}
	};
}

#endif
